#include "Header.h"

PID_Structure pid_AngleStrucuture;
PID_Structure pid_LWheelStructure;
PID_Structure pid_RWheelStructure;

float kp, ki, kd;
int16_t target;

/*pid结构体初始化*/
void PID_StructureInit(PID_Structure *pid_structure, float Kp, float Ki, float Kd)
{
    pid_structure->Kp = Kp;
    pid_structure->Ki = Ki;
    pid_structure->Kd = Kd;
    pid_structure->error_int = 0;
    pid_structure->error_present = 0;
    pid_structure->error_previous = 0;
    pid_structure->out = 0;
}

/*增量式pid*/
void PID_Increase_Cal(PID_Structure *pid_structure, float now, float target)
{
    float Kp = pid_structure->Kp;
    float Ki = pid_structure->Ki;
    float Kd = pid_structure->Kd;

    pid_structure->error_previous = pid_structure->error_present;
    pid_structure->error_present = target - now;
    if(Ki)
    {
        pid_structure->error_int += pid_structure->error_present;
    }
    else
    {
        pid_structure->error_int = 0;
    }

    pid_structure->out += Kp*pid_structure->error_present + Ki*pid_structure->error_int + Kd*(pid_structure->error_present - pid_structure->error_previous);
}

/*目标式pid*/
void PID_Target_Cal(PID_Structure *pid_structure, float now, float target)
{
    float Ki = pid_structure->Ki;
    float Kp = pid_structure->Kp;
    float Kd = pid_structure->Kd;

    pid_structure->error_previous = pid_structure->error_present;
    pid_structure->error_present = target - now;
    if(Ki)
    {
        pid_structure->error_int += pid_structure->error_present;
    }
    else
    {
        pid_structure->error_int = 0;
    }

    pid_structure->out = Kp*pid_structure->error_present + Ki*pid_structure->error_int + Kd*(pid_structure->error_present - pid_structure->error_previous);
}
